#!/usr/bin/env python
#
# *********     Servo Sequentially Write Action     *********
#
# 1-2-3舵机速度：单位步，1步0.0879度，5.21度行走1mm，52.1度行走1cm，行走1cm需要593步
# 加速度ACC：0-254
import os
from ftauto.scservo_sdk import *                    # Uses SCServo SDK library

class ServoMoveAction():
    def __init__(self):        
        # Control table address
        self.ADDR_SCS_TORQUE_ENABLE     = 40
        self.ADDR_SCS_GOAL_ACC          = 41
        self.ADDR_SCS_GOAL_POSITION     = 42
        self.ADDR_SCS_GOAL_SPEED        = 46
        self.ADDR_SCS_PRESENT_POSITION  = 56

        # Default setting
                        # SCServo ID : 1
        self.BAUDRATE                    = 1000000           # SCServo default baudrate : 1000000
        self.DEVICENAME                  = '/dev/ttyUSB0'    # Check which port is being used on your controller                                                       
        self.SCS_MINIMUM_POSITION_VALUE  = 100         # SCServo will rotate between this value
        self.SCS_MAXIMUM_POSITION_VALUE  = 4000        # and this value (note that the SCServo would not move when the position value is out of movable range. Check e-manual about the range of the SCServo you use.)
        self.SCS_MOVING_STATUS_THRESHOLD = 20          # SCServo moving status threshold
        
        self.protocol_end                = 0           # SCServo bit end(STS/SMS=0, SCS=1)
         # Initialize PortHandler instance
        # Set the port path
        # Get methods and members of PortHandlerLinux or PortHandlerWindows
        self.portHandler = PortHandler(self.DEVICENAME)

        # Initialize PacketHandler instance
        # Get methods and members of Protocol
        self.packetHandler = PacketHandler(self.protocol_end)
    
    def connected(self):
        if self.portHandler.openPort() and self.portHandler.setBaudRate(self.BAUDRATE):return True
        else:return False
        
    def currentPos(self,id):
        scs_present_position_speed, scs_comm_result, scs_error = self.packetHandler.read4ByteTxRx(self.portHandler, int(id), self.ADDR_SCS_PRESENT_POSITION)
        scs_present_position = SCS_LOWORD(scs_present_position_speed)
        scs_present_speed = SCS_HIWORD(scs_present_position_speed)
        return scs_present_position,scs_present_speed

    def move(self,ID=None,speed=None,acc=None): 
        status = 0
        SCS_ID  = int(ID)   
        SCS_MOVING_SPEED  = int(speed)          # SCServo moving speed
        SCS_MOVING_ACC = int(acc)           # SCServo moving acc    
        index = 0
        scs_goal_position = [self.SCS_MINIMUM_POSITION_VALUE, self.SCS_MAXIMUM_POSITION_VALUE]         # Goal position
       
        # Write SCServo acc
        scs_comm_result, scs_error = self.packetHandler.write1ByteTxRx(self.portHandler, SCS_ID, self.ADDR_SCS_GOAL_ACC, SCS_MOVING_ACC)
        if scs_comm_result == COMM_SUCCESS:status=1

        # Write SCServo speed
        scs_comm_result, scs_error = self.packetHandler.write2ByteTxRx(self.portHandler, SCS_ID, self.ADDR_SCS_GOAL_SPEED, SCS_MOVING_SPEED)
        if scs_comm_result == COMM_SUCCESS:status=2
        
        #当超过阈值时
        
        return status        

        # while 1:
            # print("Press any key to continue! (or press ESC to quit!)")
            # if getch() == chr(0x1b):  
                # break

            # # Write SCServo goal position
            # scs_comm_result, scs_error = self.packetHandler.write2ByteTxRx(self.portHandler, self.SCS_ID, self.ADDR_SCS_GOAL_POSITION, self.scs_goal_position[index])
            # if scs_comm_result != COMM_SUCCESS:
                # print("%s" % packetHandler.getTxRxResult(scs_comm_result))
            # elif scs_error != 0:
                # print("%s" % packetHandler.getRxPacketError(scs_error))

            # while 1:
                # # Read SCServo present position
                # scs_present_position_speed, scs_comm_result, scs_error = self.packetHandler.read4ByteTxRx(self.portHandler, self.SCS_ID, self.ADDR_SCS_PRESENT_POSITION)
                # if scs_comm_result != COMM_SUCCESS:
                    # print(packetHandler.getTxRxResult(scs_comm_result))
                # elif scs_error != 0:
                    # print(packetHandler.getRxPacketError(scs_error))

                # scs_present_position = SCS_LOWORD(scs_present_position_speed)
                # scs_present_speed = SCS_HIWORD(scs_present_position_speed)
                # print("[ID:%03d] GoalPos:%03d PresPos:%03d PresSpd:%03d" 
                      # % (SCS_ID, scs_goal_position[index], scs_present_position, SCS_TOHOST(scs_present_speed, 15)))

                # if not (abs(scs_goal_position[index] - scs_present_position_speed) > SCS_MOVING_STATUS_THRESHOLD):
                    # break


            # # Change goal position
            # if index == 0:
                # index = 1
            # else:
                # index = 0    
    def close(ID=None):
        scs_comm_result, scs_error = self.packetHandler.write1ByteTxRx(self.portHandler, ID, self.ADDR_SCS_TORQUE_ENABLE, 0)
        if scs_comm_result != COMM_SUCCESS:
            print("%s" % packetHandler.getTxRxResult(scs_comm_result))
        elif scs_error != 0:
            print("%s" % packetHandler.getRxPacketError(scs_error))
        # Close port
        portHandler.closePort()